//
//  DeviceMountedController.m
//  Podcopter
//
//  Created by Ben Gotow on 2/1/10.
//  Copyright 2010 Apple Inc. All rights reserved.
//

#import "DeviceMountedController.h"
#import "NetworkManager.h"
#import "ThrustToBrightnessMap.h"
#import "AccelerationConversions.h"
#import "DDData.h"
#import "UIImageAdditions.h"

#define testSequenceDuration 20.0
#define testSequenceInterval 1.0/60.0

#define maxAngle 75.0
#define thrustGain 2.0

@implementation DeviceMountedController

@synthesize imagePicker;

// Implement viewDidLoad to do additional setup after loading the view, typically from a nib.
- (void)viewDidLoad
{
    [super viewDidLoad];
    
    [[NetworkManager sharedManager] startServer];
    [[NSNotificationCenter defaultCenter] addObserver:self selector:@selector(peerConnected:) name:@"PeerConnected" object:nil];
    
    // register to recieve updates from the accelerometer
    [[UIAccelerometer sharedAccelerometer] setDelegate: self];
    [[UIAccelerometer sharedAccelerometer] setUpdateInterval: 1/30.0];
	
	testSequenceTimer = nil;
	testSequenceStatusLabel.text = nil;
	
	thrustMap = [LinearThrustToBrightnessMap class];
	
	pitchController = [[PIDController alloc] init];
	rollController = [[PIDController alloc] init];
	
	devicePitch = 0.0;
	deviceRoll = 0.0;
	controllerPitch = 0.0;
	controllerRoll = 0.0;
	controllerThrust = 0.0;
	
    // take the first picture in ~4 seconds
    [NSTimer scheduledTimerWithTimeInterval:4 target:self selector:@selector(takePicture) userInfo:nil repeats:NO];
    
    [imagePicker setDelegate: self];
}

- (void)viewDidUnload 
{
	if (testSequenceTimer != nil) {
		[testSequenceTimer invalidate];
		testSequenceTimer = nil;
	}
	
	[pitchController release];
	[rollController release];
	
    [super viewDidUnload];
}


#pragma mark Picture taking

- (void)takePicture
{
    [imagePicker takePicture];
}

- (void)imagePickerController:(UIImagePickerController *)picker didFinishPickingImage:(UIImage *)image editingInfo:(NSDictionary *)editingInfo
{
    CGImageRetain([image CGImage]);    
    [NSThread detachNewThreadSelector:@selector(processPicture:) toTarget:self withObject:image];
}

- (void)processPicture:(UIImage*)image
{
    NSAutoreleasePool * pool = [[NSAutoreleasePool alloc] init];
	NSData * imageData = UIImageJPEGRepresentation([image scaledImage: 0.1], 0.8);
    [connection sendData:imageData throttledToBandwidthLimit:0.25];
    [connection sendData:[NSData dataWithBytes: [@"endOfJPEG" cStringUsingEncoding: NSStringEncodingConversionAllowLossy] length:10]];
    [pool release];
    CGImageRelease([image CGImage]);
    
    [self performSelectorOnMainThread:@selector(takePicture) withObject:nil waitUntilDone:NO];
}

#pragma mark Test sequence

- (IBAction)runTestSequence:(id)sender
{
	if (testSequenceTimer != nil) {
		return;
	}
	
	testSequenceTime = 0;
	testSequenceTimer = [[NSTimer scheduledTimerWithTimeInterval:testSequenceInterval
														  target:self
														selector:@selector(updateTestSequence:)
														userInfo:nil
														 repeats:YES] retain];
	startTestSequenceButton.hidden = YES;
	stopTestSequenceButton.hidden = NO;
}

- (IBAction)stopTestSequence:(id)sender
{
	if (testSequenceTimer != nil) {
		[testSequenceTimer invalidate];
		testSequenceTimer = nil;
	}
	
	testSequenceTime = 0.0;
	
	// Reset all squares to black
	frontSquare.backgroundColor = [UIColor blackColor];
	backSquare.backgroundColor = [UIColor blackColor];
	leftSquare.backgroundColor = [UIColor blackColor];
	rightSquare.backgroundColor = [UIColor blackColor];
	
	startTestSequenceButton.hidden = NO;
	stopTestSequenceButton.hidden = YES;
	testSequenceStatusLabel.text = nil;
}

- (void)updateTestSequence:(id)sender
{
	if (testSequenceTime > testSequenceDuration) {
		[self stopTestSequence:self];
		return;
	}
	
	int currentSquare = floor(testSequenceTime / (testSequenceDuration / 4.0));
	double intensity = testSequenceTime - (currentSquare * testSequenceDuration / 4.0);
	// Scale intensity into radians, then take the sine of it
	CGFloat sineIntensity = (CGFloat)sin(intensity * M_PI / (testSequenceDuration / 4));

	switch (currentSquare)
	{
		case 0:
			frontSquare.backgroundColor = [UIColor colorWithWhite:sineIntensity alpha:1.0f];
			break;
		case 1:
			backSquare.backgroundColor = [UIColor colorWithWhite:sineIntensity alpha:1.0f];
			break;
		case 2:
			leftSquare.backgroundColor = [UIColor colorWithWhite:sineIntensity alpha:1.0f];
			break;
		case 3:
			rightSquare.backgroundColor = [UIColor colorWithWhite:sineIntensity alpha:1.0f];
			break;
		default:
			break;
	}
	
	testSequenceTime += testSequenceInterval;
	testSequenceStatusLabel.text = [NSString stringWithFormat:@"Testing square %i, %.2f/%.f sec",
									currentSquare+1, testSequenceTime, testSequenceDuration];
}

#pragma mark Receiving Data from Remote Controller

- (void)peerConnected:(NSNotification*)notif
{
    [connection release];
    connection = [[notif object] retain];
    
    // become the GamePeer's delegate so we receive callbacks when data is available
    [connection setDelegate: self];
}

- (void)gamePeerDidFailConnecting:(GamePeer *)peer
{
}

- (void)gamePeerDidConnect:(GamePeer*)peer
{
    [remoteControlStatusLabel setText: @"Connected"];
}

- (void)gamePeerDidDisconnect:(GamePeer*)peer
{
    [remoteControlStatusLabel setText: @"Disconnected"];
    [connection setDelegate: nil];
    [connection release];
    connection = nil;
}

- (void)gamePeerDidReceiveData:(NSData *)data
{
    if ([data length] >= 4 * sizeof(double)) {
        double *values = (double *)[data bytes];
		double x = values[0];
		double y = values[1];
		double z = values[2];
		double thrust = values[3];
        
		controllerPitch = calculatePitch(x, y, z);
		controllerRoll = calculateRoll(x, y, z);
		controllerThrust = thrust;
		
		NSTimeInterval now = [NSDate timeIntervalSinceReferenceDate];
		NSTimeInterval dt = now - lastControllerResponseTime;
		lastControllerResponseTime = now;
		
        framesSinceFPSUpdate ++;
        if (now - lastFPSUpdateTime > 1){
            fps = floor(framesSinceFPSUpdate / (now - lastFPSUpdateTime));
            lastFPSUpdateTime = now;
            framesSinceFPSUpdate = 0;
        }
        
		[self updateMotorOutputs:dt];

        NSString *t = [NSString stringWithFormat: @"Values from Remote:\nx:%.3f y:%.3f z:%.3f t:%.3f\n%f FPS", x, y, z, thrust, fps];
        [remoteControlStatusLabel setText: t];
	}
}


#pragma mark Thrust control

- (void)accelerometer:(UIAccelerometer *)accelerometer didAccelerate:(UIAcceleration *)a
{
    if (!baseValuesSet) {
        baseValues = [a retain];
        baseValuesSet = YES;
    }
	
	// See this article: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
	devicePitch = calculatePitch(a.x, a.y, a.z);
    [pitchValueLabel setText: [NSString stringWithFormat:@"%.3fº", devicePitch]];
	
	deviceRoll = calculateRoll(a.x, a.y, a.z);
    [rollValueLabel setText: [NSString stringWithFormat:@"%.3fº", deviceRoll]];
}

- (void)updateMotorOutputs:(double)deltaTime
{
	// Using PID controllers
	CGFloat pitch = [pitchController updateOutputWithControllerAngle:controllerPitch currentAngle:devicePitch deltaTime:deltaTime];
	CGFloat roll = [rollController updateOutputWithControllerAngle:controllerRoll currentAngle:deviceRoll deltaTime:deltaTime];
	
	CGFloat brightness;
	
	// Front/Rear
	float pitchResponse = pitch / maxAngle * thrustGain;
	float front;
	float rear;
	
	if (fabsf(pitchResponse) > controllerThrust) {
		if (pitchResponse > 0) {
			rear = 2.0f * pitchResponse;
			front = 0;
		} else {
			rear = 0;
			front = 2.0f * pitchResponse;
		}
	} else if (fabsf(pitchResponse) > 1.0f - controllerThrust) {
		if (pitchResponse > 0) {
			front = 1;
			rear = 1.0f - 2.0f * pitchResponse;
		} else {
			rear = 1;
			front = 1.0f - 2.0f * pitchResponse;
		}
	}
	
	float rearOutput = rear - fminf(front, 0) - fmaxf(front - 1.0f, 0);
	float frontOutput = front - fminf(rear, 0) - fmaxf(rear - 1.0f, 0);


	
	// Left/Right
	float rollResponse = roll / maxAngle * thrustGain;
	float left;
	float right;
	
	if (fabsf(rollResponse) > controllerThrust) {
		if (rollResponse > 0) {
			right = 2.0f * rollResponse;
			left = 0;
		} else {
			right = 0;
			left = 2.0f * rollResponse;
		}
	} else if (fabsf(rollResponse) > 1.0f - controllerThrust) {
		if (rollResponse > 0) {
			left = 1;
			right = 1.0f - 2.0f * rollResponse;
		} else {
			right = 1;
			left = 1.0f - 2.0f * rollResponse;
		}
	}
	
	float leftOutput = left - fminf(right, 0) - fmaxf(right - 1.0f, 0);
	float rightOutput = right - fminf(left, 0) - fmaxf(left - 1.0f, 0);
	
	
	
	// Front
	brightness = [thrustMap mapThrustToBrightness:frontOutput];
	frontSquare.backgroundColor = [UIColor colorWithWhite:brightness alpha:1.0f];
	
	// Rear
	brightness = [thrustMap mapThrustToBrightness:rearOutput];
	backSquare.backgroundColor = [UIColor colorWithWhite:brightness alpha:1.0f];
	
	// Left 
	brightness = [thrustMap mapThrustToBrightness:leftOutput];
	leftSquare.backgroundColor = [UIColor colorWithWhite:brightness alpha:1.0f];
	
	// Right
	brightness = [thrustMap mapThrustToBrightness:rightOutput];
	rightSquare.backgroundColor = [UIColor colorWithWhite:brightness alpha:1.0f];
}

@end

